import copy
import math
import debugpy
import matplotlib.pyplot as plt
from matplotlib.patches import Circle  # Import Circle class
import random
AGENT_MOVEMENT_CONSTANT = 0.2  # Distance the robot moves each time
MAX_SMALL_ROTATIONS = 2
MAX_LARGE_ROTATIONS = 6
SMALL_THRESHOLD = 1e-3
# Define action constants
from collections import deque


MAX_ROTATION_ATTEMPTS = 5
ACTIONS = {
    'move_ahead': 'm',
    'move_back': 'b',
    'rotate_left': 'l',
    'rotate_right': 'r',
    'rotate_left_small': 'ls',
    'rotate_right_small': 'rs',
    'end': 'end'
}
CANDIDATE_R_ACTIONS =[ rotation for rotation in range(0, 360, 6)] 


def normalize_angle(angle,mode = 'radian'):
    while angle > 180:
        angle -= 360
    while angle < -180:
        angle += 360
    if mode == 'radian':
        return math.radians(angle)
    return angle


def search_best_action(origin,target1,target2=None):

    target1 = {'x':target1[0],'z':target1[1]}
    if target2 is not None:
        target2 = {'x':target2[0],'z':target2[1]}

    min_cost = 1e9
    best_actions = None
    next_agent = None
    best_rotation = None
    for rotation in CANDIDATE_R_ACTIONS:
        if rotation == 180:
            debugpy.breakpoint()

        new_agent, actions = get_new_position(origin, rotation)
        # positon error cost
        dist_cost = cost_distance(new_agent, target1)
        cost = dist_cost

        # the orientation cost
        angle_target1 = math.degrees(math.atan2(target1['x']-new_agent['z'],target1['z']-new_agent['z']))
        angle_target1_diff = normalize_angle(angle_target1)

        if target2 is not None:
            angle_target2 = math.degrees(math.atan2(target2['x']-new_agent['x'],target2['z']-new_agent['z']))
            angle_target2_diff = normalize_angle(angle_target2)
            
        f = min(dist_cost/0.2,1)
        angle_cost = 0
        angle_cost = f *(angle_target1_diff*angle_target1_diff)+ (1-f) * (angle_target2_diff*angle_target2_diff)
        cost = cost+ angle_cost
        # cost = cost+ 1*(f *(angle_target1_diff*angle_target1_diff)+ (1-f) * (angle_target2_diff*angle_target2_diff))
        if cost < min_cost:
            min_cost = cost
            best_actions = actions
            next_agent = new_agent
            best_rotation = rotation
    print(f'best_rotation: {best_rotation}, cost: {min_cost}')
            

    return best_actions, next_agent


def cost_distance(agent1, agent2):
    return calculate_distance(agent1, agent2)


def get_new_position(origin, rotation):
    next_agent = copy.deepcopy(origin)
    actions,current_heading = get_action_from_rotation(rotation,origin['rotation'])
    dx = math.cos(math.radians(current_heading)) * AGENT_MOVEMENT_CONSTANT
    dz = math.sin(math.radians(current_heading)) * AGENT_MOVEMENT_CONSTANT
    next_agent['x'] = round(next_agent['x'] + dx, 3)
    next_agent['z'] = round(next_agent['z'] + dz, 3)
    next_agent['rotation'] = current_heading
    return next_agent, actions


def get_action_from_rotation(rotation,current_heading):
    actions = []
    # if rotation == 0:
    #     return [ACTIONS['move_ahead']],current_heading
    # if rotation == 180:
    #     return [ACTIONS['move_back']],current_heading

    if rotation > 180:
        rotation -= 360  # Normalize angle difference to (-180, 180] range
    # Large angle rotation (30°)
    while abs(rotation)>=30:
        if rotation > 0:
            actions.append(ACTIONS['rotate_right'])
            current_heading = (current_heading + 30) % 360
            rotation -= 30
        else:
            actions.append(ACTIONS['rotate_left'])
            current_heading = (current_heading - 30) % 360
            rotation += 30
    # Small angle rotation (6°)
    while abs(rotation)>=6:
        if rotation > 0:
            actions.append(ACTIONS['rotate_right_small'])
            current_heading = (current_heading + 6) % 360
            rotation -= 6
        else:
            actions.append(ACTIONS['rotate_left_small'])
            current_heading = (current_heading - 6) % 360
            rotation += 6
    actions.append(ACTIONS['move_ahead']) # rotation + move

    return actions,current_heading



def calculate_angle(current_pos, next_pos):

    dx = next_pos['x'] - current_pos['x']
    dz = next_pos['z'] - current_pos['z']
    return math.degrees(math.atan2(dx, dz)) % 360

def calculate_distance(current_pos, next_pos):

    dx = next_pos['x'] - current_pos['x']
    dz = next_pos['z'] - current_pos['z']
    return math.hypot(dx, dz)


def determine_turn_actions(current_heading, target_heading,epsilon = 0.1):

    angle_diff = (target_heading - current_heading + 360) % 360
    
    if abs(abs(angle_diff)-180) < epsilon:
        return [ACTIONS['move_back']], current_heading
    
    if angle_diff > 180:
        angle_diff -= 360  # Normalize angle difference to (-180, 180] range
    
    actions = []
    small_rotation_count = 0
    large_rotation_count = 0
    
    # Large angle rotation (30°)
    while abs(angle_diff)>=30:
        if angle_diff > 0:
            actions.append(ACTIONS['rotate_right'])
            current_heading = (current_heading + 30) % 360
            angle_diff -= 30
        else:
            actions.append(ACTIONS['rotate_left'])
            current_heading = (current_heading - 30) % 360
            angle_diff += 30
        large_rotation_count += 1
    # Small angle rotation (6°)
    while abs(angle_diff)>=6:
        if angle_diff > 0:
            actions.append(ACTIONS['rotate_right_small'])
            current_heading = (current_heading + 6) % 360
            angle_diff -= 6
        else:
            actions.append(ACTIONS['rotate_left_small'])
            current_heading = (current_heading - 6) % 360
            angle_diff += 6
        small_rotation_count += 1   
    return actions, current_heading


def determine_turn_actions_end(current_heading,angle_diff):

    if angle_diff > 180:
        angle_diff -= 360  # Normalize angle difference to (-180, 180] range
    
    actions = []
    small_rotation_count = 0
    large_rotation_count = 0
    
    # Large angle rotation (30°)
    while abs(angle_diff)>=30:
        if angle_diff > 0:
            actions.append(ACTIONS['rotate_right'])
            current_heading = (current_heading + 30) % 360
            angle_diff -= 30
        else:
            actions.append(ACTIONS['rotate_left'])
            current_heading = (current_heading - 30) % 360
            angle_diff += 30
        large_rotation_count += 1
    # Small angle rotation (6°)
    while abs(angle_diff)>=6:
        if angle_diff > 0:
            actions.append(ACTIONS['rotate_right_small'])
            current_heading = (current_heading + 6) % 360
            angle_diff -= 6
        else:
            actions.append(ACTIONS['rotate_left_small'])
            current_heading = (current_heading - 6) % 360
            angle_diff += 6
        small_rotation_count += 1   
    return actions, current_heading

def from_two_points_to_actions(current_pose, next_position):
    actions = []
    current_heading = current_pose['rotation']['y']
    target_angle = calculate_angle(current_pose, next_position)
    turn_actions, current_heading = determine_turn_actions(current_heading, target_angle)
    actions.extend(turn_actions)
    if ACTIONS['move_back'] not in turn_actions:
        actions.append(ACTIONS['move_ahead'])
    return actions

def action_from_dx_dz(dx, dz, current_heading):
    target_angle = math.degrees(math.atan2(dx, dz)) % 360
    turn_actions, current_heading = determine_turn_actions(current_heading, target_angle)
    actions = []
    actions.extend(turn_actions)
    if ACTIONS['move_back'] not in turn_actions:
        actions.append(ACTIONS['move_ahead'])
    return actions




    